/*
 *  levelUpGame - AStarPathFinder.cpp
 *  Copyright 2011 W. Reckman. All rights reserved.
 */
#include "LevelUpApp.h"
#include "AStarNode.h"
#include "AStarPathFinder.h"


/*****************************
 * CLASS PriorityQueue_NodeP *
 *****************************/

//--------------------------------------------------------------
void PriorityQueue_NodeP::clear() {
	c.clear();
}

//--------------------------------------------------------------
void PriorityQueue_NodeP::clearWithDelete() {
	for ( int i = 0; i < c.size(); ++i ) delete c[i];
	c.clear();
}

//--------------------------------------------------------------
bool PriorityQueue_NodeP::remove( AStarNode* p_ ) {
	std::vector<AStarNode*>::iterator elIt = std::find( c.begin(), c.end(), p_ );
	if ( elIt != c.end() ) {
		c.erase( elIt );
		std::make_heap( c.begin(), c.end(), AStarNodeFCompare() );
		return true;
	} else {
		return false;
	}
}

//--------------------------------------------------------------
AStarNode* PriorityQueue_NodeP::findWithPValue( const AStarNode* node_ ) {
	AStarNodeP_equal<const AStarNode> eq = { node_ };
	std::vector<AStarNode*>::iterator result = find_if( c.begin(), c.end(), eq );
	return result != c.end() ? *result : 0;
}

//--------------------------------------------------------------
const AStarNode* PriorityQueue_NodeP::operator[]( int index ) const {
	return ( index < c.size() ) ? c[index] : 0;
}


/*************************
 * CLASS AStarPathFinder *
 *************************/

//--------------------------------------------------------------
AStarPathFinder::AStarPathFinder( const LevelUpApp& appRef_ )
: appRef( appRef_ ), game( appRef_.getGame() )
{}

//--------------------------------------------------------------
//Get distance using A* with fallback to manhattan distance approximation (may be too optimistic).
int AStarPathFinder::getDistance(  const ofPoint& from_, const ofPoint& to_  ) {
	const vec_ofPoint& path = findPath( from_, to_ );
	
	if ( path.size() > 0 ) {
		int result = 0;
		
		for ( int i = 0; i < path.size() - 1; ++i ) {
			int dx = path[i + 1].x - path[i].x;
			int dy = path[i + 1].y - path[i].y;
			result += sqrtf( dx * dx + dy * dy );
		}
		
		return result;
	} else {
		return pointManhattanDist( from_, to_ );
	}
}

//--------------------------------------------------------------
//FIXME: this function probably contains memory leaks.
AStarPathFinder::vec_ofPoint AStarPathFinder::findPath( const ofPoint& from_, const ofPoint& to_, const ofPoint* otherGhost_ ) {
	static const int ghostMargin = 0, targetMargin = 3, neighbourMargin = 1; //TEMP
	
	/* first clean up the sets from last time, if any */
	
	for ( int i = 0; i < closedSet.size(); ++i ) delete closedSet[i];
	closedSet.clear();
	openSet.clearWithDelete();
	
	/* now convert the source, target and otherGhost points to nodes */
	
	AStarNode* dest = new AStarNode( GridPoint<int>( to_ ) );
	game.fillInLaneIndices( dest->getPos(), targetMargin, true );
	
	AStarNode* startingPoint = new AStarNode( GridPoint<int>( from_ ) );
	game.fillInLaneIndices( startingPoint->getPos(), ghostMargin, false );
	startingPoint->setH( startingPoint->getPos().manhattanDistanceTo( dest->getPos() ) );
	
	AStarNode* otherGhostPoint = 0;
	if ( otherGhost_ != 0 ) {
		otherGhostPoint = new AStarNode( GridPoint<int>( *otherGhost_ ) );
		game.fillInLaneIndices( otherGhostPoint->getPos(), ghostMargin, false );
	}
	
	AStarNode* foundTarget = 0;
	
	openSet.push( startingPoint );
//{ ofLog( OF_LOG_WARNING, "A* . START" ); const GridPoint<int>& p = startingPoint->getPos(); ofLog( OF_LOG_WARNING, "A* - push open (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
	
	
	/* and begin building the path */
	
	AStarNode* curr = 0;
	while ( openSet.size() > 0 ) {
		curr = openSet.top();
		openSet.pop();
		closedSet.push_back( curr );
//{ const GridPoint<int>& p = curr->getPos(); ofLog( OF_LOG_WARNING, "A* - open -> closed (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
		
		if ( curr->samePosition( *dest ) ) {
			//yay we found the best path!
			foundTarget = curr;
			break;
		}
		
		vec_AStarNodeP neighbours = getNeighbours( curr, dest, otherGhostPoint, neighbourMargin, false );
		
//ofLog( OF_LOG_WARNING, "A* . %i neighbours", neighbours.size() ); //TEMP
		vec_AStarNodeP::iterator it;
		for ( it = neighbours.begin(); it != neighbours.end(); ++it ) {
			AStarNode* candidate = *it;
			
			AStarNodeP_equal<const AStarNode> eq = { candidate };
			if ( find_if( closedSet.begin(), closedSet.end(), eq ) != closedSet.end() ) {
				delete candidate;
//{ const GridPoint<int>& p = candidate->getPos(); ofLog( OF_LOG_WARNING, "A* . neighbour already in closed  (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
				continue;
			}
			
			AStarNode* openNode = openSet.findWithPValue( candidate );
			if ( openNode == 0 ) {
				openSet.push( candidate );
//{ const GridPoint<int>& p = candidate->getPos(); ofLog( OF_LOG_WARNING, "A* - neighbour push open (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
			} else {
				if ( candidate->getG() < openNode->getG() ) {
					openSet.remove( openNode );
					delete openNode;
					openSet.push( candidate );
//{ const GridPoint<int>& p = candidate->getPos(); ofLog( OF_LOG_WARNING, "A* . neighbour replace (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
				} else {
					delete candidate;
//{ const GridPoint<int>& p = candidate->getPos(); ofLog( OF_LOG_WARNING, "A* - neighbour no replace (%i, %i)(%i, %i)", p.colIdx, p.rowIdx, p.x, p.y ); } //TEMP
				}
			}
		}
	}
	
	const vec_ofPoint& path = constructPath( foundTarget );
	
//{
//	if ( foundTarget ) {
//		const GridPoint<int>& tgp = foundTarget->getPos();
//		ofLog( OF_LOG_WARNING, "A* . END [found target (%i, %i)(%i, %i) pathLength=%i]", tgp.colIdx, tgp.rowIdx, tgp.x, tgp.y, path.size() ); //TEMP
//	} else {
//		ofLog( OF_LOG_WARNING, "A* . END [target not found]" );
//	}
//}
	
	return path;
}

//--------------------------------------------------------------
void AStarPathFinder::draw() const {
	ofPushStyle();
	ofSetLineWidth( 3 );
	
	ofSetColor( 255, 127, 127 );
	for ( int i = 0; i < openSet.size(); ++i ) {
		const AStarNode* ep = openSet[i];
		const AStarNode* sp = ( ep ) ? ep->getParent() : 0;
		const GridPoint<int>* egp = &ep->getPos();
		const GridPoint<int>* sgp = ( sp ) ? &sp->getPos() : 0;
		if ( sgp && egp ) {
			ofLine( sgp->x, sgp->y, egp->x, egp->y );
			ofDrawBitmapString( ofToString( ep->getG(), 0 ), egp->x + 10, egp->y - 7 );
			ofDrawBitmapString( ofToString( ep->getF(), 0 ), egp->x + 10, egp->y + 13 );
		}
	}
	
	ofSetColor( 127, 127, 255 );
	for ( int i = 0; i < closedSet.size(); ++i ) {
		const AStarNode* ep = closedSet[i];
		const AStarNode* sp = ( ep ) ? ep->getParent() : 0;
		const GridPoint<int>* egp = &ep->getPos();
		const GridPoint<int>* sgp = ( sp ) ? &sp->getPos() : 0;
		if ( sgp && egp ) {
			ofLine( sgp->x, sgp->y, egp->x, egp->y );
			ofDrawBitmapString( ofToString( ep->getG(), 0 ), egp->x + 10, egp->y - 7 );
			ofDrawBitmapString( ofToString( ep->getF(), 0 ), egp->x + 10, egp->y + 13 );
		}
	}
	
	ofPopStyle();
}

/*********************
 * PRIVATE FUNCTIONS *
 *********************/

//--------------------------------------------------------------
AStarPathFinder::vec_ofPoint AStarPathFinder::constructPath( AStarNode* endNode_ ) {
	vec_ofPoint result, result_inv;
	AStarNode* curr = endNode_;
	
	while ( curr ) {
		const GridPoint<int>& p = curr->getPos();
		result_inv.push_back( ofPoint( p.x, p.y ) );
		curr = curr->getParent();
	}
	
	if ( result_inv.size() > 0 ) {
		for ( int i = result_inv.size() - 1; i >= 0; --i ) {
			result.push_back( result_inv[i] );
		}
	}
	
	return result;
}

//--------------------------------------------------------------
/*
 * TODO: *********** !!! HIER !!! ***********
 * - * detect ghosts on path and increase costs of that segment in order to have ghosts avoid eachother
 *     it's probably best to have them collide too? (but how to prevent them from blocking eachother?)
 *     or just increase paths away from other ghost when they are close...so they 'flee' eachother
 */
AStarPathFinder::vec_AStarNodeP AStarPathFinder::getNeighbours( AStarNode* node_, AStarNode* tgt_, AStarNode* otherGhost_,
																															 int margin_, bool wrapVertical_ ) const {
	int canvasHeight = appRef.getViewController().getCanvasHeight();
	vec_AStarNodeP result;
	const GameModel::vec_int& cl = game.getColumnLanes();
	const GameModel::vec_int& rl = game.getRowLanes();
	const GridPoint<int>& pos = node_->getPos();
	
	if ( pos.colIdx > -1 && pos.rowIdx > -1 ) { //intersection
		if ( pos.colIdx > 0 ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx - 1], rl[pos.rowIdx], pos.colIdx - 1, pos.rowIdx ) ) );
		if ( pos.rowIdx > 0 ) {
			result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], rl[pos.rowIdx - 1], pos.colIdx, pos.rowIdx - 1 ) ) );
		} else { //add point just above top of canvas
			if ( wrapVertical_ ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], -1, pos.colIdx, -1 ) ) );
		}
		if ( pos.colIdx < cl.size() - 1 ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx + 1], rl[pos.rowIdx], pos.colIdx + 1, pos.rowIdx ) ) );
		if ( pos.rowIdx < rl.size() - 1 ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], rl[pos.rowIdx + 1], pos.colIdx, pos.rowIdx + 1 ) ) );
		if ( pos.rowIdx == rl.size() - 1 ) { //add point just below bottom of canvas
			if ( wrapVertical_ ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], canvasHeight, pos.colIdx, -1 ) ) );
			else result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], game.getStageLaneBottomY(), pos.colIdx, -1 ) ) );
		}
		
	} else if ( pos.colIdx > -1 ) {
		//on column, two neighbours (up & down)
		result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], game.getNearLane( pos, GameModel::NL_PREV_Y, false ),
																										pos.colIdx, game.getNearLane( pos, GameModel::NL_PREV_Y, true ) ) ) );
		result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], game.getNearLane( pos, GameModel::NL_NEXT_Y, false ),
																										pos.colIdx, game.getNearLane( pos, GameModel::NL_NEXT_Y, true ) ) ) );
		
		if ( pos.y < rl[0] && wrapVertical_ && pos.y > -1 ) { //from top row -> point just above top of canvas
			result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], -1, pos.colIdx, -1 ) ) );
		} else if ( pos.y < rl[0] && wrapVertical_ ) { //from just above top of canvas -> bottom row
			result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], rl[rl.size() - 1], pos.colIdx, rl.size() - 1 ) ) );
		}

		if ( pos.y > rl[rl.size() - 1] && pos.y < canvasHeight ) { //from bottom row -> point just below bottom of canvas (if wrap enabled)
			if ( wrapVertical_ ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], canvasHeight, pos.colIdx, -1 ) ) );
			else result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], game.getStageLaneBottomY(), pos.colIdx, -1 ) ) );
		} else if ( pos.y > rl[rl.size() - 1] ) { //from just below bottom of canvas -> top row
			if ( wrapVertical_ ) result.push_back( new AStarNode( GridPoint<int>( cl[pos.colIdx], rl[0], pos.colIdx, 0 ) ) );
		}
		
	} else if ( pos.rowIdx > -1 ) {
		//on row, two neighbours (left & right)
		result.push_back( new AStarNode( GridPoint<int>( game.getNearLane( pos, GameModel::NL_PREV_X, false ),
																										rl[pos.rowIdx], game.getNearLane( pos, GameModel::NL_PREV_X, true ), pos.rowIdx ) ) );
		result.push_back( new AStarNode( GridPoint<int>( game.getNearLane( pos, GameModel::NL_NEXT_X, false ),
																										rl[pos.rowIdx], game.getNearLane( pos, GameModel::NL_NEXT_X, true ), pos.rowIdx ) ) );
		
	} else {
		//not on grid...maybe add closest row & column as sole neighbour?
		ofLog( OF_LOG_WARNING, "(AStarPathFinder.cpp) node is not on grid" );
	}
	
	/* do further initialization of the points found before */
	int otherGhostEdgeIdx = ( otherGhost_ != 0 ) ? findPointInEdges( otherGhost_->getPos(), node_->getPos(), result, margin_ ) : -1;
	for ( int i = 0; i < result.size(); ++i ) {
		result[i]->setParent( node_ );
		
		ofPoint fromPos( node_->getPos().x, node_->getPos().y );
		if ( fromPos.y < 0 ) { fromPos += canvasHeight; }
		else if ( fromPos.y > canvasHeight - 1 ) { fromPos -= canvasHeight; }
		result[i]->setCost( result[i]->getPos().distanceTo( fromPos ) );
		
		//NOTE: make vertical movement cheaper to get more stable paths at intersections
		if ( fabsf( fromPos.y - result[i]->getPos().y ) > 5 ) {
			result[i]->setCost( result[i]->getCost() * 0.9f );
		}
		
		//NOTE: also make sure it's very expensive to cross an edge with the other ghost on it
		if ( i == otherGhostEdgeIdx ) {
			//result[i]->setCost( result[i]->getCost() * 100.0f );
			result[i]->setCost( CROSS_GHOST_EDGE_PENALTY );
		}
		
		ofPoint newPos( result[i]->getPos().x, result[i]->getPos().y );
		if ( newPos.y < 0 ) { newPos.y += canvasHeight; }
		else if ( newPos.y > canvasHeight - 1 ) { newPos.y -= canvasHeight; }
		result[i]->setH( tgt_->getPos().manhattanDistanceTo( newPos ) );
	}
	
	/* if the target is somewhere on the found edges, replace that edge by the target */
	int replaceIdx = findPointInEdges( tgt_->getPos(), node_->getPos(), result, margin_ );
	if ( replaceIdx > -1 ) {
		AStarNode* tgtCopy = new AStarNode( *tgt_ );
		tgtCopy->setParent( result[replaceIdx]->getParent() );
		tgtCopy->setCost( tgtCopy->getParent()->getPos().distanceTo( tgtCopy->getPos() ) );
//{
//	const GridPoint<int>& ngp = result[replaceIdx]->getPos();
//	const GridPoint<int>& tgp = tgtCopy->getPos();
//	ofLog( OF_LOG_WARNING, "A* . replacing neighbour %i (%i, %i)(%i, %i) with target (%i, %i)(%i, %i)", replaceIdx,
//				ngp.colIdx, ngp.rowIdx, ngp.x, ngp.y, tgp.colIdx, tgp.rowIdx, tgp.x, tgp.y );
//}
		delete result[replaceIdx];
		result[replaceIdx] = tgtCopy;
	}
	
	return result;
}

//--------------------------------------------------------------
int AStarPathFinder::findPointInEdges( const GridPoint<int>& p_, const GridPoint<int>& edgeSrc_,
																			const vec_AStarNodeP& edgeDests_, int margin_ ) const {
	int result = -1;
	
	//if ( p_.x == edgeSrc_.x ) {
	if ( p_.x > edgeSrc_.x - margin_ && p_.x < edgeSrc_.x + margin_ ) {
		for ( int i = 0; i < edgeDests_.size(); ++i ) {
			const GridPoint<int>& ee = edgeDests_[i]->getPos();
			int ya = ( edgeSrc_.y < ee.y ) ? edgeSrc_.y : ee.y;
			int yb = ( edgeSrc_.y < ee.y ) ? ee.y : edgeSrc_.y;
			if ( ee.x == edgeSrc_.x && p_.y >= ya && p_.y <= yb ) {
				result = i;
				break;
			}
		}
		
	//} else if ( p_.y == edgeSrc_.y ) {
		} else if ( p_.y > edgeSrc_.y - margin_ && p_.y < edgeSrc_.y + margin_ ) {
		for ( int i = 0; i < edgeDests_.size(); ++i ) {
			const GridPoint<int>& ee = edgeDests_[i]->getPos();
			int xa = ( edgeSrc_.x < ee.x ) ? edgeSrc_.x : ee.x;
			int xb = ( edgeSrc_.x < ee.x ) ? ee.x : edgeSrc_.x;
			if ( ee.y == edgeSrc_.y && p_.x >= xa && p_.x <= xb ) {
				result = i;
				break;
			}
		}
	}
	
	return result;
}
